#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/delay.h>
#include <linux/init.h>
#include <linux/module.h>
#include <linux/errno.h>
#include <linux/of_gpio.h>
#include <linux/spi/spi.h>
#include <linux/of.h>
#include <linux/platform_device.h>
#include <asm/uaccess.h>
#include <asm/io.h>
#include <linux/of_irq.h>
#include <linux/irq.h>
#include <linux/regmap.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/iio/buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include <linux/iio/imu/icm42686.h>
//#include "icm42686.h"
#include <asm/neon.h>
#include <linux/kthread.h>
#include <linux/workqueue.h>


struct workqueue_struct *wq;
struct work_struct key_ac;

struct iio_dev *icm42686dev_iio = NULL;
struct icm42686 *icm42686dev = NULL; ;
struct spi_device	*spidev_front = NULL;

struct list_head m_frame_imu_synclist;
atomic_t m_frame_imu_synclist_count = ATOMIC_INIT(0);

int m_first_imu = 0;
int m_first_imu_sync= 0;
unsigned long long  g_data_num = 0;
unsigned long long  g_pts_now = 0;
unsigned long long  g_pts_abs_now = 0;
unsigned long long  g_time_backup[TIME_RECORD_CNT] = { 0 };
unsigned long long  g_every_data_cnt_backup[DATA_RECORD_CNT] = { 0 };
unsigned long g_time_cnt = 0;
unsigned long g_data_count = 0;
u8  g_fifo_data[2048]; /**< FIFO mirroring memory area */
unsigned long long  g_last_pts;
unsigned long long  g_last_abs_pts;
int g_imu_error = 0;

/* 陀螺仪分辨率 */
const int icm42686_gyro_scale_tbl[] = {
     500U * 1000000 / 65536, 
    1000U * 1000000 / 65536,
    2000U * 1000000 / 65536,
    4000U * 1000000 / 65536,
};

/* 加速度计分辨率 */
const int icm42686_accel_scale_tbl[] = {
	 2ULL * 1000000000 / 65536,
     4ULL * 1000000000 / 65536,
     8ULL * 1000000000 / 65536,
    16ULL * 1000000000 / 65536,
    32ULL * 1000000000 / 65536,
};

static const struct iio_chan_spec icm42686_channels[] = {
    ICM42686_TEMP_CHAN(IIO_TEMP, ICM42686_SCAN_TEMP),
    ICM42686_CHAN(IIO_ANGL_VEL, IIO_MOD_X, ICM42686_SCAN_GYRO_X),
    ICM42686_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, ICM42686_SCAN_GYRO_Y),
    ICM42686_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, ICM42686_SCAN_GYRO_Z),
    ICM42686_CHAN(IIO_ACCEL, IIO_MOD_X, ICM42686_SCAN_ACCEL_X),
    ICM42686_CHAN(IIO_ACCEL, IIO_MOD_Y, ICM42686_SCAN_ACCEL_Y),
    ICM42686_CHAN(IIO_ACCEL, IIO_MOD_Z, ICM42686_SCAN_ACCEL_Z),
};

unsigned long long imu_get_time_us(void)
{
    struct timespec64 time;
    ktime_get_real_ts64(&time);

    return (unsigned long long)((time.tv_sec * 1000000LLU) + time.tv_nsec/1000);
}

static ssize_t
spidev_sync_front(struct spi_device *spidev, struct spi_message *message)
{
	int status;
	struct spi_device *spi;

	spi = spidev;
	if (spi == NULL)
		status = -ESHUTDOWN;
	else
		status = spi_sync(spi, message);

	if (status == 0)
		status = message->actual_length;

	return status;
}

static ssize_t icm42686_transfer_read(struct spi_device *spidev, u8 reg_addr, const u8 *reg_data, u32 cnt, u8 fifo_mode)
{
    ssize_t			status = 0;

    u8 tx_buf[1] = {reg_addr | 0x80};
    u8 rx_buf[800] ;        

    struct spi_transfer	t = {
        .rx_buf		= rx_buf,
        .tx_buf		= tx_buf,
        .len		= cnt+1,
        .speed_hz	= 24000000,
        .bits_per_word = 8,
        // .delay_usecs = 0,
    };
    struct spi_message	m;

    spi_message_init(&m);
    spi_message_add_tail(&t, &m);

    status = spidev_sync_front(spidev, &m);
    if (status > 0) {
        memcpy((void*)reg_data, rx_buf+1, cnt);
        /*printk("f %lld, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x\n", system_time,
        rx_buf[1]<<8|rx_buf[2], rx_buf[3]<<8|rx_buf[4], rx_buf[5]<<8| rx_buf[6],
        rx_buf[7]<<8|rx_buf[8], rx_buf[9]<<8|rx_buf[10], rx_buf[11]<<8|rx_buf[12]);*/
        return cnt;
    }

    return 0;
}

int fifo_add_frame_imu_synclist(unsigned long long time_us, unsigned long long times_ms,
                                unsigned long long sys_time_ms, struct icm42686_data * imu_new) 
{
    FRAME_IMU_SYNC_INFO* syncinfo;

    syncinfo =
        (FRAME_IMU_SYNC_INFO*)kmalloc(sizeof(FRAME_IMU_SYNC_INFO), GFP_ATOMIC);
    if (syncinfo == NULL) {
        printk("syncinfo malloc fail \n");
        return -1;
    }

    memset(syncinfo, 0, sizeof(FRAME_IMU_SYNC_INFO)); 
    syncinfo->accesscount = 0;
    syncinfo->app_frame_imu_one_synclist.imu_time_us = time_us;
    
    syncinfo->app_frame_imu_one_synclist.frame_timestamp = times_ms; 
    //printk("systimes  :%llu\n", sys_time_ms);
    syncinfo->app_frame_imu_one_synclist.frame_systimestamp =sys_time_ms;

    memcpy(&syncinfo->app_frame_imu_one_synclist.imu_frontdata,
                imu_new,
                sizeof(syncinfo->app_frame_imu_one_synclist.imu_frontdata));

    memcpy(&syncinfo->app_frame_imu_one_synclist.imu_bottomdata,
                imu_new,
                sizeof(syncinfo->app_frame_imu_one_synclist.imu_bottomdata));

    list_add_tail(&syncinfo->list, &m_frame_imu_synclist);
    atomic_inc(&m_frame_imu_synclist_count);

    return 0;
}

static void icm42686_get_time_inter(unsigned long long *time_inter,unsigned long long record_num )
{
    int  i;
    unsigned long long adddatacnt = 0;

    if (g_data_count < DATA_RECORD_CNT) {
        g_every_data_cnt_backup[g_data_count] = record_num;
    } else {
        for (i = 0; i < DATA_RECORD_CNT - 1; i++) {
            g_every_data_cnt_backup[i] = g_every_data_cnt_backup[i + 1];
        }

        g_every_data_cnt_backup[DATA_RECORD_CNT - 1] = record_num;
    }

    if (g_time_cnt < TIME_RECORD_CNT) {
        g_time_backup[g_time_cnt] = g_pts_abs_now ;

        for (i = 0; i <= g_data_count; i++) {
            adddatacnt += g_every_data_cnt_backup[i];
        }

        *time_inter = div_u64(g_time_backup[g_time_cnt] - g_time_backup[0], div_0_to_1(adddatacnt));
    } else {
        for (i = 0; i < TIME_RECORD_CNT - 1; i++) {
            g_time_backup[i] = g_time_backup[i + 1];
        }

        g_time_backup[TIME_RECORD_CNT - 1] = g_pts_abs_now ;

        for (i = 1; i < DATA_RECORD_CNT; i++) {
            adddatacnt += g_every_data_cnt_backup[i];
        }

        *time_inter = div_u64(g_time_backup[TIME_RECORD_CNT - 1] - g_time_backup[0], div_0_to_1(adddatacnt));
    }

    if (g_data_count < DATA_RECORD_CNT) {
        g_data_count++;
    }
}

static inline ssize_t
spidev_sync_front_fifo_read(struct spi_device *spidev, struct icm42686_data * imu, size_t len)
{
	uint16_t packet_count_i, packet_count, total_packet_count = 0;
	uint16_t packet_size = FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE 
                            + TEMP_DATA_SIZE + TIME_DATA_SIZE ;
    uint16_t fifo_idx = 0, invalid_frame_cnt = 0;
    int i =0 ;
    fifo_header_t * header;
    u8 int_status;
    u8 data[2] = {0}; /* 2 byte buf */
    u64 time_inter = 0;
    unsigned long long  new_times =0,new_sys_times = 0;

    if(g_imu_error > 100) {
        //printk("imu fifo total_packet_count  total error is  lager \n");
        return 0;
    } 

    //第一次调度FIFO需要清空
    if(m_first_imu == 0) {
        /* fifo flush */
        regmap_write(icm42686dev->regmap, SIGNAL_PATH_RESET, 0x02);
        udelay(10);
        
        g_pts_now = imu_get_time_us();
        g_pts_abs_now =  ktime_to_us(ktime_get());;
        g_time_backup[g_time_cnt] = g_pts_abs_now;
        g_last_pts  = g_pts_now;
        g_last_abs_pts  =  g_pts_abs_now ;
        g_time_cnt++;
        m_first_imu = 1;

        return 0;
    }  
    
    g_pts_now = imu_get_time_us();      
    g_pts_abs_now =  ktime_to_us(ktime_get());

    if(m_first_imu_sync == 0 &&  (g_pts_now > 1000000000000) ) {
            printk("m_first_imu_sync ok \n");
            g_pts_now = imu_get_time_us();
            g_last_pts  =  g_pts_now ;
            m_first_imu_sync = 1;
            return 0;
    }

    if ( 0 == icm42686_transfer_read(spidev, INT_STATUS, &int_status,1,0)) {
        printk("front fifo_read fail \n");
        return 0;
    }

    if ( 0 == icm42686_transfer_read(spidev, FIFO_COUNTH , &data[0],2,0)) {
        printk("front fifo_read fail \n");
        return 0;
    }

    total_packet_count = (uint16_t)(data[1] | (data[0] << 8));
    packet_count       = total_packet_count;

    if(total_packet_count == 0) {
        // printk("imu fifo total_packet_count  is 0 \n");
        return 0;
    }

    if(packet_count > 30) {
        packet_count = 30;
        total_packet_count = packet_count;
        printk("imu fifo total_packet_count  is lager \n");
        regmap_write(icm42686dev->regmap, SIGNAL_PATH_RESET, 0x02);
        udelay(10);
        ++g_imu_error;         
    }
    else {
        if(g_imu_error > 0)
            --g_imu_error;
    }

    for(i = 0; i < packet_count; i++)
        icm42686_transfer_read(spidev, 0x30, g_fifo_data + i * packet_size, packet_size, 0);
           
    //printk("time = %llu\n", g_pts_abs_now);
    icm42686_get_time_inter(&time_inter, total_packet_count);

    for (packet_count_i = 0; packet_count_i < packet_count; packet_count_i++) {
        struct icm42686_data imu_data = {0};
        header = (fifo_header_t *)&g_fifo_data[fifo_idx];
        fifo_idx += FIFO_HEADER_SIZE;

        if (header->Byte == 0x80) {
            u8 is_invalid_frame = 1;

            /* Check N-FIFO_HEADER_SIZE remaining bytes are all 0 to be invalid frame */
            for (i = 0; i < (packet_size - FIFO_HEADER_SIZE); i++) {
                if (g_fifo_data[fifo_idx + i]) {
                    is_invalid_frame = 0;
                    break;
                }
            }
            /* In case of invalid frame read FIFO will be retried for this packet */
            invalid_frame_cnt += is_invalid_frame;
            fifo_idx += packet_size - FIFO_HEADER_SIZE;
        }
        else {
            if (header->bits.msg_bit) {
                /* MSG BIT set in FIFO header, Resetting FIFO */
                /* fifo flush */
                regmap_write(icm42686dev->regmap, SIGNAL_PATH_RESET, 0x02);
                udelay(10);
                return 0;
            }

            if (header->bits.accel_bit) {
                imu_data.acc.x = g_fifo_data[0 + fifo_idx] << 8 | g_fifo_data[1 + fifo_idx];
                imu_data.acc.y= g_fifo_data[2 + fifo_idx] << 8 | g_fifo_data[3 + fifo_idx];
                imu_data.acc.z = g_fifo_data[4 + fifo_idx] << 8 | g_fifo_data[5 + fifo_idx];
            }

            fifo_idx += FIFO_ACCEL_DATA_SIZE;

            if (header->bits.gyro_bit) {
                imu_data.gyro.x = g_fifo_data[0 + fifo_idx] << 8 | g_fifo_data[1 + fifo_idx];
                imu_data.gyro.y = g_fifo_data[2 + fifo_idx] << 8 | g_fifo_data[3 + fifo_idx];
                imu_data.gyro.z =  g_fifo_data[4 + fifo_idx] << 8 | g_fifo_data[5 + fifo_idx];
            }

            fifo_idx += FIFO_GYRO_DATA_SIZE;
            fifo_idx += TEMP_DATA_SIZE;
            fifo_idx += TIME_DATA_SIZE; 

            g_last_pts = g_last_pts + time_inter ;
            g_last_abs_pts  =    g_last_abs_pts   + time_inter;

            if (g_time_cnt < TIME_RECORD_CNT) {
                g_time_backup[g_time_cnt] =   g_last_abs_pts ;
                g_time_cnt++;
            }
            else {
                g_time_backup[TIME_RECORD_CNT - 1] =  g_last_abs_pts   ;
            }

            new_times = div_u64(g_last_abs_pts, 1000);
            new_sys_times = div_u64(g_last_pts , 1000);
            imu_data.sys_timestamp = g_last_pts;
            fifo_add_frame_imu_synclist(g_last_abs_pts, new_times, new_sys_times, &imu_data);
            /*printk("f abs us:%llu, abs ms:%llu acc x:%04d, acc y:%04d,, acc z:%04d,, gyro x:%04d,
                gyro :y%04d,, gyro z:%04d,   %lu\n", g_last_abs_pts, new_sys_times,
                imu_data.acc.x ,imu_data.acc.y, imu_data.acc.z,
                imu_data.gyro.x , imu_data.gyro.y , imu_data.gyro.z,g_time_cnt);*/
        }
    }

	return 0;
}

// static struct icm42686_data * ac_imu = NULL;
struct icm42686_data fr_imu;

/* imu read interface */
int icm42686_get_data_front(struct iio_dev * iio, struct icm42686_data * imu)
{	
    // ac_imu = imu;
	schedule_work(&key_ac);

	return 0;
}

/* work queue */
static void work_queue_icm42686(struct work_struct *arg)
{
    if (spidev_front != NULL)
        spidev_sync_front_fifo_read(spidev_front, &fr_imu, 0);
    else
        printk("spidev_front is null.\n");
}

EXPORT_SYMBOL(fr_imu);
EXPORT_SYMBOL(icm42686dev_iio);
EXPORT_SYMBOL(spidev_front);
EXPORT_SYMBOL(icm42686_get_data_front);

EXPORT_SYMBOL(m_frame_imu_synclist_count);
EXPORT_SYMBOL(m_frame_imu_synclist);

static void icm42686_reg_fifo_init(struct icm42686 *dev)
{
    printk("42686 fifo init \n");

    regmap_write(dev->regmap, INTF_CONFIG0, 0x70);
    regmap_write(dev->regmap, FIFO_CONFIG1, 0x67);
    regmap_write(dev->regmap, FIFO_CONFIG2, 0x5);
    regmap_write(dev->regmap, INT_SOURCE0, 0x06);
    /* stream-to-fifo mode */
    regmap_write(dev->regmap, FIFO_CONFIG, 0x40);
    /* TMST_TO_REGS_EN:1,TMST_FSYNC_EN:1,TMST_EN:1 */
    regmap_write(dev->regmap, TMST_CONFIG, 0x22);
}

/*
 * icm42686内部寄存器初始化函数
 * @param   : none
 * @return  : noreturn
 */
static void icm42686_reg_init(struct icm42686 *dev)
{
    int ret;

    regmap_write(dev->regmap, REG_BANK_SEL, 0x00);
    mdelay(50);
     //复位
    regmap_write(dev->regmap, DEVICE_CONFIG, 0x01);
    mdelay(50);

    if (regmap_read(dev->regmap, WHO_AM_I, &ret) >= 0)
        printk("--- icm42686 ID = %#x ---\r\n", ret);
	/*
	* GYRO_CONFIG0:Gyro Full scale and sampling rate config
	* GYRO_FS_SEL[bit5-7],GYRO_ODR[bit0-3],page 58 of 105
	* GYRO_FS_SEL:
		0x00:+-4000dps(default); 0x02:+-2000dps; 0x40:+-1000dps;0x60:+-500dps 
	* GYRO_ODR:
		0x00:Reserved; 0x01:32KHz; 0x02:16KHz; 0x03:8KHz; 0x04:4KHz; 
		0x05:2KHz; 0x06:1KHz(default); 0x07:200Hz; 0x08:100Hz; 
	*/
    regmap_write(dev->regmap, GYRO_CONFIG0,  0x26);	//+-2000dps//1KHz

	/*
	* GYRO_CONFIG1: address 0x51; default:0x16
	* TEMP_FILT_BW:[bit5-7],GYRO_UI_FILT_ORD:[bit2-3],GYRO_DEC2_M2_ORD:[bit0-1]
	* default value: DLPF BW=4000Hz,DLPF Latency=0.125ms
	*	UI filt ord:2th order; gyro_dec2_m2_ord:3th order
	*/
    regmap_write(dev->regmap, GYRO_CONFIG1,  0x16);

	/*
	* ACCEL_CONFIG0
	* ACCEL_FS_SEL[bit5-7] and ACCEL_ODR[bit0-3]) address=0x50,value default 0x06
	* ACCEL_FS_SEL: 000: +-32g; 001:+-16g; 010:+-8g; 011:+-4g; 100:+-2g; 
	* ACCEL_ODR:0001:32KHz; 0010:16KHz; 0011: 8KHz; 0100: 4KHz;0101: 2KHz;0110: 1KHz;
	*	0111: 200Hz; 1000: 1000Hz; 1001:50Hz; 1010:25Hz 
	*/
    regmap_write(dev->regmap, ACCEL_CONFIG0,  0x06);/* full scale: +-2g,accel sampling rate: 1KHz*/

    /*
	* ACCEL_CONFIG1:order for ACCEL_UI_FILT_ORD[bit3-4] and ACCEL_DEC2_M2_ORD [bit1-2]
	* Address:0x53,default value:0x0D
	* ACCEL_UI_FILT_ORD: 00:1th ; 01:2th; 10:3th; 11:reserved 
	* ACCEL_DEC2_M2_ORD: 00:1th ; 01:2th; 10:3th; 11:reserved 
	*/
	regmap_write(dev->regmap, ACCEL_CONFIG1,  0x0D);

	/*
	* GYRO_ACCEL_CONFIG0: bandwidth for accel LPF and Gyro LPF; 
	* Address=0x52,default value:0x11; 
	* ACCEL_UI_FILT_BW:[bit4-7],default:1,BW=max(400Hz,ODR)/4
	* GYRO_UI_FILT_BW:[bit0-3],default:1,BW=max(400Hz,ODR)/4
	*/
	regmap_write(dev->regmap, GYRO_ACCEL_CONFIG0, 0x11);//1st order	P76; R-> 0x52=0x00

	/* 
	* INT1:active low,open drain,pulsed mode
	* INT2:active low,open drain,pulsed mode
	*/
	regmap_write(dev->regmap, INT_CONFIG, 0x00);

	/* stream-to-fifo mode */
	regmap_write(dev->regmap, FIFO_CONFIG, 0x40);
	
	/* Disables de-assert duration, Otherwise, output 4K frequency */
	regmap_write(dev->regmap, INT_CONFIG1, 0x60);	

	/* TMST_TO_REGS_EN:1,TMST_FSYNC_EN:1,TMST_EN:1 */
	regmap_write(dev->regmap, TMST_CONFIG, 0x13);

	/* FIFO count in records pakage of data,
	FIFO count and data are reported in big endian format  
	*/
	regmap_write(dev->regmap, INTF_CONFIG0, 0x70);

	/* set UI data ready interrupt touted to INT1 */
	regmap_write(dev->regmap, FIFO_SOURCE0, 0x08);

	/*
	* FIFO_CONFIG1: Address=0x5F,default value:0x00; 
	* bit0:FIFO_ACCEL_EN; 
	* bit1:FIFO_GYRO_EN; 
	* bit2:FIFO_TEMP_EN; 
	*/
	regmap_write(dev->regmap, FIFO_CONFIG1, FIFO_CONFIG1_DATA);//GYRO and ACCEL FIFO enable	R-> 0x5f=0x03

	/*
	* PWR_MGMT0: Address=0x4E,default value:0x00; 
	* ACCEL_MODE[bit0-1]:11:Places accelerometer in Low Noise (LN) Mode
	* GYRO_MODE[bit2-3]:11:Places gyroscope in Low Noise (LN) Mode
	*/

    icm42686_reg_fifo_init(dev);

	regmap_write(dev->regmap, PWR_MGMT0, 0xf);	// 1st order	P72	R->0x4e=0x0f
}

/*
 * @brief 读取icm42686传感器数据
 * 
 * @param dev icm42686设备
 * @param reg 要读取通道寄存器首地址
 * @param ch  通道序号
 * @param val 保存读取的值
 * @return    0, 成功;  其它值, 错误
 */
static int icm42686_sensor_read(struct icm42686 *dev, int reg, int ch, int *val)
{
    int ind, result;
    __be16 d;

    ind = (ch - IIO_MOD_X) * 2;
    result = regmap_bulk_read(dev->regmap, reg + ind, (u8 *)&d, 2);
    if (result)
        return -EINVAL;

    *val = (short)be16_to_cpup(&d);
	printk("--- 0x%02x 0x%02x ---\n", *val, d);

    return IIO_VAL_INT;
}

static int icm42686_read_raw(struct iio_dev *indio_dev,
			      struct iio_chan_spec const *chan,
			      int *val, int *val2,
			      long mask)
{
    int ret;
    struct icm42686 *dev = iio_priv(indio_dev);

    switch (mask) {
    case IIO_CHAN_INFO_RAW: /* 读取传感器数据 */
        switch (chan->type) {
        case IIO_ANGL_VEL:  /* 读取陀螺仪数据 */
            mutex_lock(&dev->lock);
            ret = icm42686_sensor_read(dev, GYRO_OUT_ADDR, chan->channel2, val);
            mutex_unlock(&dev->lock);
            break;

        case IIO_ACCEL:  /* 读取加速度计数据 */
            mutex_lock(&dev->lock);
            ret = icm42686_sensor_read(dev, ACCEL_OUT_ADDR, chan->channel2, val);
            mutex_unlock(&dev->lock);
            break;

        case IIO_TEMP:  /* 读取温度数据 */
            mutex_lock(&dev->lock);
            ret = icm42686_sensor_read(dev, TEMP_OUT_ADDR, chan->channel2, val);
            mutex_unlock(&dev->lock);
            break;

        default:
            ret = -EINVAL;
            break;
        }
        break;

    case IIO_CHAN_INFO_OFFSET:
        switch (chan->type) {
        case IIO_TEMP:
            mutex_lock(&dev->lock);
            *val = 0;
            ret = IIO_VAL_INT;
            mutex_unlock(&dev->lock);
            break;

        default:
            ret = -EINVAL;
            break;
        }
        break;

    case IIO_CHAN_INFO_SCALE:
        switch (chan->type) {
        case IIO_TEMP:
            mutex_lock(&dev->lock);
            *val = 326800000 / 1000000;
            *val2 = 326800000 % 1000000;
            ret = IIO_VAL_INT_PLUS_MICRO;
            mutex_unlock(&dev->lock);
            break;
            
        case IIO_ANGL_VEL:
            mutex_lock(&dev->lock);
            regmap_read(dev->regmap, GYRO_CONFIG0, &ret);
            *val = 0;
            *val2 = icm42686_gyro_scale_tbl[(ret & 0x18) >> 3];
            ret = IIO_VAL_INT_PLUS_MICRO;
            mutex_unlock(&dev->lock);
            break;
        
        case IIO_ACCEL:
            mutex_lock(&dev->lock);
            regmap_read(dev->regmap, ACCEL_CONFIG0, &ret);
            *val = 0;
            *val2 = icm42686_accel_scale_tbl[(ret & 0x18) >> 3];
            printk("Read accel scale index value: %d\n", icm42686_accel_scale_tbl[(ret & 0x18) >> 3]);
            ret = IIO_VAL_INT_PLUS_NANO;
            mutex_unlock(&dev->lock);
            break;
        
        default:
            ret = -EINVAL;
            break;
        }
        break;

    default:
        ret = -EINVAL;
        break;
    }
    
    return ret;
}

static int icm42686_write_raw(struct iio_dev *indio_dev,
			 struct iio_chan_spec const *chan,
			 int val,
			 int val2,
			 long mask)
{
    int ret = 0;
    int i, d;
    struct icm42686 *dev = iio_priv(indio_dev);
    
    switch (mask) {
    case IIO_CHAN_INFO_SCALE:
        switch (chan->type) {
        case IIO_ANGL_VEL:
            mutex_lock(&dev->lock);
            for(i = 0; i < ARRAY_SIZE(icm42686_gyro_scale_tbl); ++i) 
                if (icm42686_gyro_scale_tbl[i] == val2) 
                    break;

            if (i < ARRAY_SIZE(icm42686_gyro_scale_tbl)) {
                d = (i << 3);
                ret = regmap_write(dev->regmap, GYRO_CONFIG0, d);
            } else
                ret = -EINVAL;
            mutex_unlock(&dev->lock);
            break;

        case IIO_ACCEL:
            mutex_lock(&dev->lock);
            for(i = 0; i < ARRAY_SIZE(icm42686_accel_scale_tbl); ++i) 
                if (icm42686_accel_scale_tbl[i] == val2) 
                    break;

            if (i < ARRAY_SIZE(icm42686_accel_scale_tbl)) {
                d = (i << 3);
                ret = regmap_write(dev->regmap, ACCEL_CONFIG0, d);
            } else
                ret = -EINVAL;
            mutex_unlock(&dev->lock);
            break;

        default:
            ret = -EINVAL;
            break;
        }
        break;
   
    default:
        ret = -EINVAL;
        break;
    }
    return ret;
}

static int icm42686_write_raw_get_fmt(struct iio_dev *indio_dev,
			 struct iio_chan_spec const *chan,
			 long mask)
{
    switch (mask) {
    case IIO_CHAN_INFO_SCALE:
        switch (chan->type) {
        case IIO_TEMP:
        case IIO_ANGL_VEL:
            return IIO_VAL_INT_PLUS_MICRO;

        case IIO_ACCEL:
            return IIO_VAL_INT_PLUS_NANO;
        
        default:
            return IIO_VAL_INT_PLUS_MICRO;
        }

    default:
        return IIO_VAL_INT_PLUS_MICRO;
    }
    return -EINVAL;
}

static const struct iio_info icm42686_info = {
    .read_raw = &icm42686_read_raw,
    .write_raw = &icm42686_write_raw,
    .write_raw_get_fmt = &icm42686_write_raw_get_fmt,
};

/*
 * spi驱动的probe函数, 当驱动与设备匹配会执行此函数
 * @param   client: spi设备
 * @param   id:     spi设备ID
 */
static int icm42686_probe(struct spi_device *spi)
{
    int ret = 0;
    
    struct iio_dev *iiodev;
    #if 0
    struct icm42686_data imu_test = {0};
    #endif

    printk("--- icm42686 front probe 1---\n");

    /* 1.向内核申请分配iio_dev内存, 包括同时分配的icm42686_dev内存 */
    iiodev = devm_iio_device_alloc(&spi->dev, sizeof(struct icm42686));
    if (!iiodev) {
        return -ENOMEM;
    }
    
	spi_set_drvdata(spi, iiodev);
	icm42686dev_iio = spi_get_drvdata(spi);

    /* 2.把已分配的indio_dev内存结构的私有数据赋给icm42686_dev */
    icm42686dev = iio_priv(iiodev);
    icm42686dev->spi = spi;
    mutex_init(&icm42686dev->lock);

	spidev_front = spi;	

    /* 3.设置iio_dev的主要成员变量 */
    iiodev->name = "icm42686";
    iiodev->dev.parent = &spi->dev;
    iiodev->info = &icm42686_info;
    iiodev->modes = INDIO_DIRECT_MODE;
    iiodev->channels = icm42686_channels;
    iiodev->num_channels = ARRAY_SIZE(icm42686_channels);

    /* 4.注册iio_dev */
    ret = iio_device_register(iiodev);
    if (ret < 0) {
        dev_err(&spi->dev, "iio_device_register failed\n");
        goto err_iio_register;
    }        

    /* 5.初始化regmap_config配置 */
    icm42686dev->regmap_cfg.reg_bits = 8;           /* 寄存器长度 */
    icm42686dev->regmap_cfg.val_bits = 8;           /* 值长度 */
    icm42686dev->regmap_cfg.read_flag_mask = 0x80;  /* 读掩码 */
	icm42686dev->regmap_cfg.use_hwlock = 1;
	icm42686dev->regmap_cfg.hwlock_mode = 1;
	icm42686dev->regmap_cfg.fast_io = 1;

    /* 6.初始化SPI接口的regmap */
    icm42686dev->regmap = regmap_init_spi(spi, &icm42686dev->regmap_cfg);
    if (IS_ERR(icm42686dev->regmap)) {
        ret = PTR_ERR(icm42686dev->regmap);
        goto err_regmap_init;
    }

    /* 初始化spi_device */
    spi->mode = SPI_MODE_0;
    spi->bits_per_word = 8;
    spi->max_speed_hz = 24000000;
    ret = spi_setup(spi);
    icm42686dev->spi = spi;
    //spi_set_drvdata(spi, icm42686dev);

    /* 初始化icm42686内部寄存器 */
    icm42686_reg_init(icm42686dev);

	INIT_WORK(&key_ac, &work_queue_icm42686);
    
    #if 0
    while (1) {
        icm42686_get_data_front(icm42686dev_iio, &imu_test);
    }
    #endif

    return 0;

err_regmap_init:
    iio_device_unregister(iiodev);
err_iio_register:
    //kzfree(icm42686dev);
    regmap_exit(icm42686dev->regmap);

    return ret;
}

/**
 * spi驱动的remove函数，移除spi驱动的时候此函数会执行
 * @param   : client spi设备
 * @return  : 0 成功; 负值 失败
 */
static void icm42686_remove(struct spi_device *spi)
{    
    struct iio_dev *iiodev = spi_get_drvdata(spi);

    /* 删除regmap */
    regmap_exit(icm42686dev->regmap);

    /* 注销IIO */
    iio_device_unregister(iiodev);
    //kzfree(icm42686dev);
}

static const struct spi_device_id icm42686_id[] = {
    {"tdk,icm42686", 0},
    {}
};
MODULE_DEVICE_TABLE(spi, icm42686_id);

static const struct of_device_id icm42686_of_match[] = {
    {.compatible = "tdk,icm42686"},
    { /* Sentinel */}
};
MODULE_DEVICE_TABLE(of, icm42686_of_match);

static struct spi_driver icm42686_driver = {
    .probe  = icm42686_probe,
    .remove = icm42686_remove,
    .driver = {
        .owner = THIS_MODULE,
        .name  = "icm42686",
        .of_match_table = icm42686_of_match,
    },
    .id_table = icm42686_id,
};

static int __init icm42686_init(void)
{
	printk("--- icm42686_front_init ---\n");
    return spi_register_driver(&icm42686_driver);
}

static void __exit icm42686_exit(void)
{
	icm42686dev_iio = NULL;
	spidev_front = NULL;
    spi_unregister_driver(&icm42686_driver);
}

module_init(icm42686_init);
module_exit(icm42686_exit);

MODULE_LICENSE("GPL");
MODULE_AUTHOR("ac");


